#include "mavlink_interface.hpp"
#include <sys/ioctl.h>
#include <stdio.h>
#include <unistd.h>
#include <bot_core/bot_core.h>

namespace asctec_sdk_driver {

/////////////////////////////////////////////////////////////////////////////
// Sending data from the quad over LCM
/////////////////////////////////////////////////////////////////////////////

void publish_imu_message(lcm::LCM* lcm, const mavlink_asctec_imu_t * imu,
                         int64_t recv_time) {

  static const double g = 9.806;
  ins_t msg;
  msg.utime = recv_time;

  // THIS SHOULD PUT THE OUTPUT INS MESSAGE IN FRONT-LEFT-UP CONFIGURATION
  msg.accel[0] = g * imu->acc_x / 10000.0;  //-
  msg.accel[1] = g * imu->acc_y / 10000.0;  //+
  msg.accel[2] = g * imu->acc_z / 10000.0;  //+
  msg.gyro[0] = bot_to_radians(imu->angvel_roll*0.0154);
  msg.gyro[1] = bot_to_radians(imu->angvel_pitch*0.0154);  //xgyro in .0154 degrees/s
  msg.gyro[2] = bot_to_radians(imu->angvel_yaw*0.0154);
  msg.mag[0] = imu->Hx;  // not sure these are in the correct frame
  msg.mag[1] = imu->Hy;
  msg.mag[2] = imu->Hz;
  msg.pressure = imu->fusion_height / 1000.0;
  msg.device_time = imu->msec;

  double rpy[3];
  rpy[0] = bot_to_radians(imu->angle_roll/100.0);
  rpy[1] = bot_to_radians(imu->angle_pitch/100.0);
  rpy[2] = bot_to_radians(imu->angle_yaw/100.0);

  bot_roll_pitch_yaw_to_quat(rpy, msg.quat);
  lcm->publish("ASCTEC_INS", &msg);
}

void publish_gps_message(lcm::LCM* lcm, const mavlink_asctec_gps_t * gps,
                         int64_t recv_time) {

  gps_data_t msg;
  msg.utime = recv_time;
  msg.elev = gps->GPS_height;
  msg.gps_time = gps->msec;
  msg.latitude = gps->GPS_latitude * 1e-7;
  msg.longitude = gps->GPS_longitude * 1e-7;
  msg.horizontal_accuracy = gps->GPS_position_accuracy * 1e-3;
  msg.vertical_accuracy = gps->GPS_height_accuracy * 1e-3;

  double gps_xy_speed[2] = { gps->GPS_speed_x * 1e-3, gps->GPS_speed_y * 1e-3 };

  msg.speed = bot_vector_magnitude_2d(gps_xy_speed);
  msg.heading = bot_to_radians(gps->GPS_heading*1e-3);
  msg.gps_lock = gps->GPS_status & 0x0001;
  msg.numSatellites = gps->GPS_sat_num;

  lcm->publish("ASCTEC_GPS_DATA", &msg);
}

void publish_rc_channels_message(lcm::LCM* lcm,
                                 const mavlink_asctec_rc_data_t * rc_channels,
                                 int64_t recv_time) {

  rc_data_t msg;
  msg.utime = recv_time;
  msg.num_channels = 8;
  msg.channels.resize(msg.num_channels);
  for (int i = 0; i < 8; i++)
    msg.channels[i] = rc_channels->value[i];
  msg.lock = rc_channels->lock;

  lcm->publish("ASCTEC_RC_DATA", &msg);
}

inline float asctec_motor_val_to_rpm(float val) {
  return (25. + (175. * val) / 200.) * 40.;
}

void publish_motor_rpm_message(lcm::LCM* lcm,
                               const mavlink_asctec_motor_rpm_t * rpms,
                               int64_t recv_time) {
  motor_rpm_t msg;
  msg.utime = recv_time;
  msg.num_motors = 4;
  msg.motor_rpm.resize(msg.num_motors);
  for (int i = 0; i < msg.num_motors; i++)
    msg.motor_rpm[i] = asctec_motor_val_to_rpm(rpms->motor[i]);
  lcm->publish("ASCTEC_MOTOR_RPMS", &msg);
}

void publish_heartbeat_message(lcm::LCM* lcm,
                               const mavlink_asctec_heartbeat_t * heartbeat,
                               int64_t recv_time, int64_t host_time) {

  heartbeat_t msg;
  msg.utime = recv_time;
  msg.host_utime = host_time;
  msg.sdk_msec = heartbeat->msec;
  msg.sdk_status = heartbeat->sdk_status;
  msg.control_mode = heartbeat->control_mode;
  msg.publishing_mode = heartbeat->publishing_mode;
  msg.cpu_load = heartbeat->cpu_load;
  lcm->publish("ASCTEC_SDK_HEARTBEAT", &msg);
}

/////////////////////////////////////////////////////////////////////////////
// Sending data to the quad
/////////////////////////////////////////////////////////////////////////////

void send_stick_commands(int fd, const quad_control_msg_t * control_msg) {
  mavlink_message_t msg;
  uint8_t buf[MAVLINK_MAX_PACKET_LEN];

  //yaw pitch roll should be [-1 1], and thrust is [0 1]
  int16_t roll = static_cast<int16_t>(control_msg->roll * 2047.0);
  int16_t pitch = static_cast<int16_t>(control_msg->pitch * 2047.0);
  int16_t yaw = static_cast<int16_t>(control_msg->yaw * 2047.0);
  uint16_t thrust = static_cast<int16_t>(control_msg->thrust * 4095.0);

  mavlink_msg_asctec_stick_command_pack(0, 1, &msg, roll, pitch, yaw, thrust);

  uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);

  int ret = write(fd, buf, len);
  //  printf("wrote %d of %d\n", ret, len);
}

void send_motor_commands(int fd, const motor_commands_t * motor_command) {
  mavlink_message_t msg;
  uint8_t buf[MAVLINK_MAX_PACKET_LEN];

  //TODO: what's the convention for this?
  uint32_t usec = static_cast<uint32_t>(motor_command->utime);
  uint8_t roll = static_cast<uint8_t>(motor_command->roll);
  uint8_t pitch = static_cast<uint8_t>(motor_command->pitch);
  uint8_t yaw = static_cast<uint8_t>(motor_command->yaw);
  uint8_t thrust = static_cast<uint8_t>(motor_command->thrust);

  mavlink_msg_asctec_motor_command_pack(0, 1, &msg, usec, roll, pitch, yaw,
                                        thrust);

  uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);

  int ret = write(fd, buf, len);
}

void send_api_command(int fd,
                      const mavlink_asctec_api_command_t * api_command) {
  mavlink_message_t msg;
  uint8_t buf[MAVLINK_MAX_PACKET_LEN];

  mavlink_msg_asctec_api_command_pack(0, 1, &msg, api_command->control_mode,
                                      api_command->publishing_mode,
                                      api_command->imu_pub_period,
                                      api_command->motor_rpm_pub_period,
                                      api_command->rc_data_pub_period,
                                      api_command->gps_data_pub_period);

  uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);

  int ret = write(fd, buf, len);
}

}  // namespace asctec_sdk_driver
